#include <ros/ros.h>
#include <iostream>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>

using namespace std;
using namespace cv;

void drawRect(Mat frame,RotatedRect temp);
Point getRotateRectCenter(RotatedRect temp1);
Point3d CornerPoint(Point3f point3D,Mat rotateMatrix, Mat tvec);

int main(int argc,char** argv)
{
    uint32_t shape = visualization_msgs::Marker::CUBE;
    ros::init(argc, argv, "show");
    ros::NodeHandle n1;
    ros::Rate looplate(60);
    ros::Publisher publish = n1.advertise<visualization_msgs::Marker>("Marker",1);
    sensor_msgs::ImagePtr msg;
    visualization_msgs::Marker marker;
    marker.header.frame_id = "/my_frame";
    marker.header.stamp = ros::Time::now();
    marker.ns = "basic_shapes";
    marker.id = 1;
    marker.type = shape;

    Mat frame;
    Mat Image1;
    Mat Image2;

    //相机内参
    Mat cameraMatrix;
    Mat distCoeffs;

    //相机外参
    double s;
    Mat rotateMatrix;   //相机旋转矩阵
    Mat tvec;
    Mat rvec;
    
    //计算四个角点像素坐标所要用到的数据
    RotatedRect rotate_Rect;
    Rect rect;
    vector<Mat> channels;
    vector<vector<Point>> contours;
    vector<vector<Point>> contours1;
    vector<Vec4i> hierarchy;
    vector<RotatedRect> rotate_Rect_Group;
    vector<Rect> rect_Group;

    //相机坐标系下四个角点坐标
    Point3d left_up3D,left_down3D,right_down3D,right_up3D;
    
    //solvepnp算法所需的点集
    vector<Point3f> Point3Ds;
    vector<Point2f> Point2Ds, testPoint;
    Point left_up,left_down,right_up,right_down,center1,center2;

    while(ros::ok())
    {
	VideoCapture video("../water.avi");
	while(1)
    {
        // 对帧进行异常检测
		if (!video.read(frame)) break;
        video.read(frame);
        GaussianBlur(frame, Image1, Size(3,3),0);
        split(Image1, channels);
        Image1 = channels.at(2)-channels.at(0);   //红色通道减去蓝色通道
        double th = threshold(Image1, Image2, 40, 255, THRESH_BINARY|THRESH_OTSU);
        if(th-10>0)
        {
            threshold(Image1, Image2, th-10, 255, THRESH_BINARY);
        }
        findContours(Image2, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_NONE);
        //drawContours(frame, contours, -1, Scalar(255,0,0),2);   //画出阈值化处理后的轮廓
        for(int i =0; i<contours.size();i++)
        {
            float contours_area = contourArea(contours[i]);
            if(contours_area<100 || contours_area>3000) continue;
            rotate_Rect = minAreaRect(contours[i]);
            rect = boundingRect(Mat(contours[i]));
            rotate_Rect_Group.push_back(rotate_Rect);
            rect_Group.push_back(rect);
            contours1.push_back(contours[i]);
            rectangle(frame, rect, Scalar(0,255,0),2);
        }
        for(int i = 0; i < contours1.size(); i++)
        {
            for(int j = 0; j < contours1.size(); j++)
            {
                center1 = getRotateRectCenter(rotate_Rect_Group[i]);
                center2 = getRotateRectCenter(rotate_Rect_Group[j]);
                if(center1.x < center2.x)
                {
                    if(rotate_Rect_Group[i].size.width > rotate_Rect_Group[i].size.height)
                    {
                        left_up.x   = center1.x + cos(rotate_Rect_Group[i].angle/180 * CV_PI)*rotate_Rect_Group[i].size.width/2;
                        left_down.x = center1.x - cos(rotate_Rect_Group[i].angle/180 * CV_PI)*rotate_Rect_Group[i].size.width/2;
                        left_up.y   = center1.y - abs(sin(rotate_Rect_Group[i].angle/180 * CV_PI))*rotate_Rect_Group[i].size.width/2;
                        left_down.y = center1.y + abs(sin(rotate_Rect_Group[i].angle/180 * CV_PI))*rotate_Rect_Group[i].size.width/2;
                    }
                    else
                    {
                        left_up.y   = center1.y - cos(rotate_Rect_Group[i].angle/180 * CV_PI)*rotate_Rect_Group[i].size.height/2;
                        left_down.y = center1.y + cos(rotate_Rect_Group[i].angle/180 * CV_PI)*rotate_Rect_Group[i].size.height/2;
                        left_up.x   = center1.x - abs(sin(rotate_Rect_Group[i].angle/180 * CV_PI))*rotate_Rect_Group[i].size.height/2;
                        left_down.x = center1.x + abs(sin(rotate_Rect_Group[i].angle/180 * CV_PI))*rotate_Rect_Group[i].size.height/2;
                    
                    }
                    if(rotate_Rect_Group[j].size.width > rotate_Rect_Group[j].size.height)
                    {
                        right_up.x  = center2.x + cos(rotate_Rect_Group[j].angle/180 * CV_PI)*rotate_Rect_Group[j].size.width/2;
                        right_down.x= center2.x - cos(rotate_Rect_Group[j].angle/180 * CV_PI)*rotate_Rect_Group[j].size.width/2;
                        right_up.y  = center2.y - abs(sin(rotate_Rect_Group[j].angle/180 * CV_PI))*rotate_Rect_Group[j].size.width/2;
                        right_down.y= center2.y + abs(sin(rotate_Rect_Group[j].angle/180 * CV_PI))*rotate_Rect_Group[j].size.width/2;
                    }
                    else
                    {
                        right_up.y  = center2.y - cos(rotate_Rect_Group[j].angle/180 * CV_PI)*rotate_Rect_Group[j].size.height/2;
                        right_down.y= center2.y + cos(rotate_Rect_Group[j].angle/180 * CV_PI)*rotate_Rect_Group[j].size.height/2;
                        right_up.x  = center2.x - abs(sin(rotate_Rect_Group[j].angle/180 * CV_PI))*rotate_Rect_Group[j].size.height/2;
                        right_down.x= center2.x + abs(sin(rotate_Rect_Group[j].angle/180 * CV_PI))*rotate_Rect_Group[j].size.height/2;
                    }
                    break;
                    }
                else
                {

                }
            }
        }
        circle(frame, right_down,5,Scalar(255,255,255));
        circle(frame, left_down, 5, Scalar(255,255,255));
        circle(frame, right_up, 5, Scalar(255,255,255));
        circle(frame, left_up, 5, Scalar(255,255,255));
        //line(frame, left_down, right_up, Scalar(255,255,255), 2);
        //line(frame, left_up, right_down, Scalar(255,255,255), 2);

        //相机内参,畸变参数定义:
        cameraMatrix = Mat(3,3,CV_64F,Scalar::all(0));
        distCoeffs   = Mat(1,5,CV_64F,Scalar::all(0));
        //赋值
        cameraMatrix.at<double>(0,0) = 1128.048344;
        cameraMatrix.at<double>(0,2) = 339.421769;
        cameraMatrix.at<double>(1,1) = 1127.052190;
        cameraMatrix.at<double>(1,2) = 236.535242;
        cameraMatrix.at<double>(2,2) = 1.;

        distCoeffs.at<double>(0,0) = -0.568429;
        distCoeffs.at<double>(0,1) = 0.514592;
        distCoeffs.at<double>(0,2) = -0.000126;
        distCoeffs.at<double>(0,3) = 0.000500;
        distCoeffs.at<double>(0,4) = 0.000000;

        //设定世界坐标系(装甲板坐标系
        Point3Ds.push_back(cv::Point3f(-67.5f,-27.5f,0));
        Point3Ds.push_back(cv::Point3f( 67.5f,-27.5f,0));
        Point3Ds.push_back(cv::Point3f( 67.5f, 27.5f,0));
        Point3Ds.push_back(cv::Point3f(-67.5f, 27.5f,0));

        //角点像素坐标
        Point2Ds.push_back(left_up);
        Point2Ds.push_back(right_up);
        Point2Ds.push_back(right_down);
        Point2Ds.push_back(left_down);

        //solvePnP求得旋转向量和平移向量
        solvePnP(Point3Ds, Point2Ds, cameraMatrix, distCoeffs, rvec, tvec);

        //使用projectPoints函数来将3D坐标转换成2D坐标
        projectPoints(Point3Ds, rvec, tvec, cameraMatrix, distCoeffs, testPoint);
        for(int i = 0; i<testPoint.size(); i++)
        {
            circle(frame, testPoint[i], 3, Scalar(255,0,0));
        }
        imshow("frame",frame);
        waitKey(30);
        //罗德里格斯公式:将旋转向量变为旋转矩阵
        Rodrigues(rvec, rotateMatrix);
        //通过旋转矩阵求解旋转角
        float theta_z = abs(atan2(rotateMatrix.at<double>(1,0), rotateMatrix.at<double>(0,0))*57.2958);

        float theta_y = abs(atan2(-rotateMatrix.at<double>(2,0), sqrt(rotateMatrix.at<double>(2,0) * rotateMatrix.at<double>(2,0) + rotateMatrix.at<double>(2,2) * rotateMatrix.at<double>(2,2)))*57.2958);

        float theta_x = abs(atan2(rotateMatrix.at<double>(2,1), rotateMatrix.at<double>(2,2))*57.2958);
        
        //计算装甲板在相机坐标系下的坐标
        left_up3D = CornerPoint(Point3Ds[0],rotateMatrix, tvec);
        right_up3D = CornerPoint(Point3Ds[1],rotateMatrix, tvec);
        right_down3D = CornerPoint(Point3Ds[2],rotateMatrix, tvec);
        left_down3D = CornerPoint(Point3Ds[3],rotateMatrix, tvec);

        //设置标记的位姿
        marker.pose.position.x = (left_up3D.x + right_down3D.x); 
        marker.pose.position.y = (left_up3D.y + right_down3D.y);
        marker.pose.position.z = (left_up3D.z + right_down3D.z);
        marker.pose.orientation.x = theta_x/180 * CV_PI;   
        marker.pose.orientation.y = theta_y/180 * CV_PI;  
        marker.pose.orientation.z = theta_z/180 * CV_PI;  
        marker.pose.orientation.w = 1.0;

        //设置标记的长宽高
        marker.scale.x = 135;
        marker.scale.y = 55;
        marker.scale.z = 10;

        //设置标记的颜色和不透明度
        marker.color.r = 1.0f;
        marker.color.g = 1.0f;
        marker.color.b = 1.0f;
        marker.color.a = 1.0;    //不透明度

        publish.publish(marker);
        ros::spinOnce();
        looplate.sleep();
        Point3Ds.clear();
        Point2Ds.clear();
        rotate_Rect_Group.clear();
        rect_Group.clear();
        contours.clear();
        contours1.clear();
    }
    }
}

Point getRotateRectCenter(RotatedRect temp1)
{
    Point2f edge_points[4];
    temp1.points(edge_points);
    return Point((edge_points[0].x + edge_points[2].x)/2,(edge_points[0].y + edge_points[2].y)/2);
}

Point3d CornerPoint(Point3f point3D, Mat rotateMatrix, Mat tvec)
{
    Mat xyz(3,1,CV_64F,Scalar::all(0));
    xyz.at<double>(0,0) = point3D.x;
    xyz.at<double>(1,0) = point3D.y;
    xyz.at<double>(2,0) = point3D.z;

    Mat temp = rotateMatrix*xyz+tvec;
    return Point3d(temp.at<double>(0,0),temp.at<double>(1,0),temp.at<double>(2,0));
}
